/**

This file is part of MaCI/GIMnet.

MaCI/GIMnet is free software: you can redistribute it and/or modify it 
under the terms of the GNU Lesser General Public License as published 
by the Free Software Foundation, either version 3 of the License, or 
(at your option) any later version.

MaCI/GIMnet is distributed in the hope that it will be useful, but WITHOUT 
ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or 
FITNESS FOR A PARTICULAR PURPOSE.  See the GNU Lesser General Public 
License for more details.

You should have received a copy of the GNU Lesser General Public 
License along with GIMnet. (See COPYING.LESSER) If not, see 
<http://www.gnu.org/licenses/>.

**/
#include "World.h"
#include "LaserScanner.h"
#include "owndebug.h"


CLaserScanner::CLaserScanner(CWorld *World, CObject *ParentObject, 
      dBodyID parentBody, dReal *posOffset, dMatrix3 rotOffset, float interval, 
      unsigned int numOfRays, float range, float minAngle, float maxAngle, 
      MaCI::MaCICtrl::CMaCICtrlServer *mcs, int minor, 
      std::string interfaceName)
  :CObject(World,ParentObject),
  PosOffset(posOffset),
  interval(interval),
  timer(0),
  rangingServer(mcs,minor),
  numberOfRays(numOfRays),
  range(range)
{
  for (int i=0; i<12;i++)
    this->RotOffset[i]=rotOffset[i];

  RaySpaceID=dSimpleSpaceCreate(0);

  rayCluster=new CRayCluster(this,this->RaySpaceID,parentBody,posOffset,
                            rotOffset,numOfRays,range,minAngle,maxAngle);
  
  rayCluster->IsVisible=false;
  
  rayCluster->NotifyCollisions=true;

  const std::vector<TRayInfo*>* ri=rayCluster->GetRayInfo();
  rangingData.resize(numberOfRays);
  for (unsigned int i=0;i<numberOfRays;i++)
    rangingData[i].angle=(*ri)[i]->angle;

  rangingServer.SetDeviceLimits(0,range);
  rangingServer.SetInterfaceInstanceName(interfaceName);
  fprintf(stderr,"INSTANCE NAME=%s\n",interfaceName.c_str());
  //TODO
  //rangingServer.SetDevicePosition(,,,);


}

CLaserScanner::~CLaserScanner()
{
  delete rayCluster;
}

void CLaserScanner::Draw()
{
  
  glPopMatrix();
  glPopAttrib();
  rayCluster->Draw();
}

void CLaserScanner::Activate()
{
  rangingServer.Open();

}

void CLaserScanner::Deactivate()
{
  rangingServer.Close();
}

void CLaserScanner::Update(float StepSize)
{
  timer+=StepSize;
  if (timer>=interval)
  {
    GetReading();
    rangingServer.SendDistanceArrayEvent(rangingServerData);
    timer=0;
  }
}

void CLaserScanner::OnCollision(CPrimitive *primitive, int numc, dContact *contacts, CPrimitive *primitive2)
{

}

void CLaserScanner::GetReading()
{
  for (unsigned int i=0;i<numberOfRays;i++)
    rangingData[i].distance=-1;

  rayCluster->Set();

  dSpaceCollide2((dGeomID)RaySpaceID,(dGeomID)SpaceID,this,&RayCallback);
  
  // Also collide the rays with all static objects (= WALLS)
  dSpaceCollide2((dGeomID)RaySpaceID,(dGeomID)StaticSpaceID,this,&RayCallback); 

  rangingServerData.Reset();

  rangingServerData.EncodeTo(&binbag);

  //float t=SimWorld->GetSimulationTime();
  rangingServerData.SetTimestamp(MaCI::Common::TTimestamp());
  rangingServerData.SetDistanceHeader(MaCI::Ranging::TDistanceHeader(0,numberOfRays));
  for (unsigned int i=0;i<numberOfRays;i++)
  {
    MaCI::Ranging::TDistance d;
    d.angle=rangingData[i].angle;
    d.distance=rangingData[i].distance;
    rangingServerData.AddDistance(d);
  }
}


void CLaserScanner::RayCallback(void *data, dGeomID o1, dGeomID o2)
{
  CLaserScanner *scanner=(CLaserScanner*) data;

  dContact contact;
  if (dCollide(o1, o2, 1, &contact.geom, sizeof(dContact)))
  {
    dGeomID g;
    if (dGeomGetClass(contact.geom.g1)==dRayClass) {
      g=contact.geom.g1;
    } else if (dGeomGetClass(contact.geom.g2)==dRayClass) {
      g=contact.geom.g2;
    } else {
      dPrint(1,"Error: no ray types passed to collision callback!");
      return;
    }

    TRayInfo *ri=(TRayInfo*)((TGeomData*)dGeomGetData(g))->userData;
    
    /*IMPORTANT!: ray collision detection returns many collisions, but not on same callback call
    so remember to choose the shortest distance value!!
    */

    if (scanner->rangingData[ri->index].distance==-1 || scanner->rangingData[ri->index].distance>contact.geom.depth)
    {
      scanner->rangingData[ri->index].distance=contact.geom.depth;
      scanner->rangingData[ri->index].angle=ri->angle;
    }

  }
}

void CLaserScanner::SetDevicePosition(float x, float y, float z, float heading)
{
  rangingServer.SetDevicePosition(x,y,z,heading);
}


